// -----------------------------
// Author: 		Dhaenens Gert	
// Creation: 	1/4/2014 8:56:42 PM				
// -----------------------------

#include "Quaternion.h"
#include "Vector3.h"
#include "Vector4.h"
#include "Matrix.h"
#include "CMath.h"

/* Constructors */
Quaternion::Quaternion()
	: x(0.0f)
	, y(0.0f)
	, z(0.0f)
	, w(1.0f)
{
}

Quaternion::Quaternion(Float X, Float Y, Float Z, Float W)
	: x(X)
	, y(Y)
	, z(Z)
	, w(W)
{
}

Quaternion::Quaternion(const Quaternion& refQuaternion)
	: x(refQuaternion.x)
	, y(refQuaternion.y)
	, z(refQuaternion.z)
	, w(refQuaternion.w)
{
}

Quaternion& Quaternion::operator=(const Quaternion& refQuaternion)
{
	x = refQuaternion.x;
	y = refQuaternion.y;
	z = refQuaternion.z;
	w = refQuaternion.w;
	return (*this);
}


/* Equality Operators */
Bool Quaternion::operator==(const Quaternion& refQuaternion) const
{
	return (x == refQuaternion.x) &&
		(y == refQuaternion.y) &&
		(z == refQuaternion.z) &&
		(w == refQuaternion.w);
}

Bool Quaternion::operator!=(const Quaternion& refQuaternion) const
{
	return !((*this) == refQuaternion);
}

Bool Quaternion::operator<(const Quaternion& refQuaternion) const
{
	return (x < refQuaternion.x) &&
		(y < refQuaternion.y) &&
		(z < refQuaternion.z) &&
		(w < refQuaternion.w);
}

Bool Quaternion::operator>(const Quaternion& refQuaternion) const
{
	return (x > refQuaternion.x) &&
		(y > refQuaternion.y) &&
		(z > refQuaternion.z) &&
		(w > refQuaternion.w);
}

Bool Quaternion::operator<=(const Quaternion& refQuaternion) const
{
	return (x <= refQuaternion.x) &&
		(y <= refQuaternion.y) &&
		(z <= refQuaternion.z) &&
		(w <= refQuaternion.w);
}

Bool Quaternion::operator>=(const Quaternion& refQuaternion) const
{
	return (x >= refQuaternion.x) &&
		(y >= refQuaternion.y) &&
		(z >= refQuaternion.z) &&
		(w >= refQuaternion.w);
}


/* Mathematical Out-Of-Place Operators */
Quaternion Quaternion::operator*(const Quaternion& refQuaternion) const
{
	Quaternion q;
	q.x = (w * refQuaternion.x + x * refQuaternion.w + y * refQuaternion.z - z * refQuaternion.y);
	q.y = (w * refQuaternion.y - x * refQuaternion.z + y * refQuaternion.w + z * refQuaternion.x);
	q.z = (w * refQuaternion.z + x * refQuaternion.y - y * refQuaternion.x + z * refQuaternion.w);
	q.w = (w * refQuaternion.w - x * refQuaternion.x - y * refQuaternion.y - z * refQuaternion.z);
	return q;
}
Vector3 Quaternion::operator*(const Vector3& refVector) const
{
	Vector4 v(refVector, 0.0f);
	v = (*this) * v;
	return Vector3(v);
}
Vector4 Quaternion::operator*(const Vector4& refVector) const
{
	Matrix m = this->ToMatrix();
	return (refVector * m);
}


/* Methods */
Quaternion Quaternion::Normalized() const
{
	Quaternion q(*this);
	Float fMagnitude = Math::Sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
	q.x /= fMagnitude;
	q.y /= fMagnitude;
	q.z /= fMagnitude;
	q.w /= fMagnitude;
	return q;
}

Quaternion Quaternion::Conjugate() const
{
	Quaternion q(*this);
	q.x = -q.x;
	q.y = -q.y;
	q.z = -q.z;
	return q;
}

Matrix Quaternion::ToMatrix() const
{
	//Matrix m = Matrix( Vector4( w*w + x*x - y*y - z*z , 2.0f*x*y - 2.0f*w*z , 2.0f*x*z + 2.0f*w*y , 0.0f ),
	//				   Vector4( 2.0f*x*y + 2.0f*w*z , w*w-x*x+y*y-z*z , 2.0f*y*z + 2.0f*w*x , 0.0f ),
	//				   Vector4( 2.0f*x*z - 2.0f*w*y , 2.0f*y*z - 2.0f*w*x , w*w-x*x-y*y+z*z , 0.0f ),
	//				   Vector4( 0.0f , 0.0f , 0.0f , 1.0f ));

	Float xx = x * x;
	Float xy = x * y;
	Float xz = x * z;
	Float xw = x * w;

	Float yy = y * y;
	Float yz = y * z;
	Float yw = y * w;

	Float zz = z * z;
	Float zw = z * w;

	Float mat[16];

	mat[0] = 1.0f - 2.0f * (yy + zz);
	mat[1] = 2.0f * (xy - zw);
	mat[2] = 2.0f * (xz + yw);

	mat[4] = 2.0f * (xy + zw);
	mat[5] = 1.0f - 2.0f * (xx + zz);
	mat[6] = 2.0f * (yz - xw);

	mat[8] = 2.0f * (xz - yw);
	mat[9] = 2.0f * (yz + xw);
	mat[10] = 1.0f - 2.0f * (xx + yy);

	mat[3] = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0.0f;
	mat[15] = 1.0f;

	return Matrix(mat);
}

void Quaternion::AsArray(Float arrFloat[4]) const
{
	arrFloat[0] = x;
	arrFloat[1] = y;
	arrFloat[2] = z;
	arrFloat[3] = w;
}


/* Static Methods */
Quaternion Quaternion::FromEulerAngles(const Vector3& vAxis, Float fAngle)
{
	Float c = Math::Cos(fAngle / 2.0f);
	Float s = Math::Sin(fAngle / 2.0f);
	Quaternion q;
	q.x = vAxis.x * s;
	q.y = vAxis.y * s;
	q.z = vAxis.z * s;
	q.w = c;
	q = q.Normalized();
	return q;
}

Quaternion Quaternion::FromEulerAngles(const Vector4& vAxis, Float fAngle)
{
	return Quaternion::FromEulerAngles(Vector3(vAxis), fAngle);
}